import time
import struct
import threading
from utils_py.configure import MotorCfg
from ctypes import *
import time
import os
VCI_USBCAN2 = 4
STATUS_OK = 1

class VCI_INIT_CONFIG(Structure):  
    _fields_ = [("AccCode", c_uint),
                ("AccMask", c_uint),
                ("Reserved", c_uint),
                ("Filter", c_ubyte),
                ("Timing0", c_ubyte),
                ("Timing1", c_ubyte),
                ("Mode", c_ubyte)
                ]  
class VCI_CAN_OBJ(Structure):  
    _fields_ = [("ID", c_uint),
                ("TimeStamp", c_uint),
                ("TimeFlag", c_ubyte),
                ("SendType", c_ubyte),
                ("RemoteFlag", c_ubyte),
                ("ExternFlag", c_ubyte),
                ("DataLen", c_ubyte),
                ("Data", c_ubyte*8),
                ("Reserved", c_ubyte*3)
                ] 
 
# 获取当前文件的绝对路径
current_file_path = os.path.abspath(__file__)
# 获取当前文件所在的文件夹路径
current_dir = os.path.dirname(current_file_path)
CanDLLName = f'{current_dir}/../3rdParty/ControlCAN.so' 
canDLL = cdll.LoadLibrary(f'{current_dir}/../3rdParty/libcontrolcan.so')



class MotorWaist:
    def __init__(self):
        # self.baudrate = baudrate
        self.lock_serial = threading.Lock()  # 互斥锁
        self.lock_msg_m1 = threading.Lock()  # 互斥锁
        self.lock_msg_m2 = threading.Lock()  # 互斥锁
        # self.lock_running_m1 = threading.Lock()  # 互斥锁
        # self.lock_running_m2 = threading.Lock()  # 互斥锁
        self.open_device()
        self.init_can(0) # init can1
        self.running = [False, False]  # 控制发送线程的运行状态
        self.can_obj_motor1 = []
        self.can_obj_motor1.append(self.create_loc_controll_can_obj(id=1, pos=0., spd=3, cur=10, ack=2))
        self.can_obj_motor2 = []
        self.can_obj_motor2.append(self.create_loc_controll_can_obj(id=2, pos=0., spd=3, cur=10, ack=2))
        self.thread_m1 = threading.Thread(target=self.maintain_motor_brake, args=(1, self.can_obj_motor1, self.lock_msg_m1, 200))
        self.thread_m2 = threading.Thread(target=self.maintain_motor_brake, args=(2, self.can_obj_motor2, self.lock_msg_m2, 200))

    def open_device(self):
        ret = canDLL.VCI_OpenDevice(VCI_USBCAN2, 0, 0)
        if ret == STATUS_OK:
            print('调用 VCI_OpenDevice成功\r\n')
        if ret != STATUS_OK:
            print('调用 VCI_OpenDevice出错\r\n')

    def init_can(self, can_id = 0):
    
        vci_initconfig = VCI_INIT_CONFIG(0x80000008, 0xFFFFFFFF, 0,
                                 0, 0x00, 0x14, 0)#波特率125k，正常模式
        ret = canDLL.VCI_InitCAN(VCI_USBCAN2, 0, can_id, byref(vci_initconfig))
        if ret == STATUS_OK:
            print(f'CAN{can_id} init successful\r\n')
        if ret != STATUS_OK:
            print(f'CAN{can_id} init failed\r\n')
        
        ret = canDLL.VCI_StartCAN(VCI_USBCAN2, 0, can_id)
        if ret == STATUS_OK:
            print(f'CAN{can_id} start successful\r\n')
        if ret != STATUS_OK:
            print(f'CAN{can_id} start failed\r\n')

    def motor_on(self, id): 
        """电机使能"""
        
        if id == 1:
            if not self.thread_m1.is_alive():
                self.thread_m1.start()
            else:
                self.running[0] = True
        elif id == 2:
            if not self.thread_m2.is_alive():
                self.thread_m2.start()
            else:
                self.running[1] = True
        else:
            raise ValueError("id must be 1 or 2")
    
    def motor_off(self, id):
        """电机失能"""
        self.running[id-1] = False

    def close(self):
        """关闭串口"""
        canDLL.VCI_CloseDevice(VCI_USBCAN2, 0) 

    def send(self, data):
        """发送数据"""
        ret = canDLL.VCI_Transmit(VCI_USBCAN2, 0, 0, byref(data), 1)

    def create_loc_controll_can_obj(self, id, pos, spd, cur, ack):
        """创建伺服位置控制模式的CAN报文"""
        motor_mode = 0x01  # 电机模式为0x01（表示伺服位置控制模式）
        pos_bytes = struct.pack('<f', pos)  # 期望位置（32位浮点数，单位：度）
        spd = int(spd * 10)  # 期望速度（uint15，单位：rpm/10）
        cur = int(cur * 10)  # 电流阈值（uint12，单位：A/10）
        ack = ack & 0x03  # 报文返回状态（uint2）
        pos_int = struct.unpack('<I', pos_bytes)[0]  # 转换为无符号整数（4字节）
        message_data = (motor_mode << 61) | (pos_int << 29) | (spd << 14) | (cur << 2) | ack
        hex_str = hex(message_data)[2:]
        hex_tuple = tuple(int(hex_str[i:i+2], 16) for i in range(0, len(hex_str), 2))
        # byte_array = bytes.fromhex(hex_str[2:])
        # int_array = list(byte_array)
        ubyte_array = c_ubyte*8
        data = ubyte_array(*hex_tuple)
        ubyte_3array = c_ubyte*3
        reserved = ubyte_3array(0, 0 , 0)
        vci_can_obj = VCI_CAN_OBJ(id, 0, 0, 1, 0, 0,  8, data, reserved)#单次发送
        return vci_can_obj

    def parse_motor_response(self,response):
        """解析返回的报文内容"""
        if len(response) != 8:
            raise ValueError("报文长度必须为 8 字节")

        # 解析报文类和错误信息
        byte0 = response[0]
        message_class = (byte0 >> 5) & 0x07  # 取高3位
        error_code = byte0 & 0x1F  # 取低5位

        # 解析电机位置（float32）
        pos_bytes = bytes(response[1:5])
        pos = struct.unpack('<f', pos_bytes)[0]  # 小端序解析

        # 解析实际电流（int16）
        current_bytes = bytes(response[5:7])
        current = int.from_bytes(current_bytes, byteorder='little', signed=True)

        # 解析电机温度（uint8）
        temperature = response[7]

        return {
            "报文类": message_class,
            "错误信息": error_code,
            "电机位置": pos,
            "实际电流": current,
            "电机温度": temperature,
        }
    
    def generate_motor_query_command(self):
        """
        生成电机查询指令的字节流

        参数:
            mode (int): 电机模式 (uint3, 0~7)
            reserved (int): 保留数据域 (uint5, 0~31)
            query_code (int): 查询代码 (uint8, 0~255)

        返回:
            bytes: 生成的指令字节流
        """
        # 检查输入范围
        mode = 0x07  # 电机模式 (uint3)
        reserved = 0x00  # 保留数据域 (uint5)
        query_code = 0x01  # 查询代码 (uint8)

        if not (0 <= mode <= 7):
            raise ValueError("电机模式必须为 0~7 的整数")
        if not (0 <= reserved <= 31):
            raise ValueError("保留数据域必须为 0~31 的整数")
        if not (0 <= query_code <= 255):
            raise ValueError("查询代码必须为 0~255 的整数")

        # 将电机模式 (uint3) 和保留数据域 (uint5) 组合成一个字节
        combined_byte = (mode << 5) | reserved

        # 生成完整的指令字节流
        command_bytes = bytes([combined_byte, reserved,reserved,query_code])
        return command_bytes
    
    def inquire_position(self, id):
        """伺服位置控制模式"""
        CAN_msg = self.generate_motor_query_command()
        byte_data = self.create_serial_msg(id, CAN_msg)
        self.send(byte_data)
        print(f"Send: {byte_data}")
        a=self.receive()
        print(a)
    
    def maintain_motor_brake(self, id, can_obj, lock:threading.Lock, freq=200):
        """以XXXHz频率发送数据"""
        self.running[id-1] = True
        while True:
            if not self.running[id-1]:
                continue
            t1 = time.time()
            with lock:
            #    print(f"Send data: {msg[0]}\n")
               self.send(can_obj[0])
            t2 = time.time()
            sleepTime = 1./freq - (t2-t1)
            sleepTime = sleepTime if sleepTime > 0 else 0
            # print(sleepTime)
            time.sleep(sleepTime)

    def set_motor_position(self, id, pos=0., spd=3., cur=10, ack=2):
        """设置电机位置"""
        # 根据id的值，设置对应电机的位置、速度、电流和确认信息
        if id == 1:
            # 使用锁机制，防止多线程同时操作
            with self.lock_msg_m1:
                # 设置电机1的位置、速度、电流和确认信息
                self.can_obj_motor1[0]=self.create_loc_controll_can_obj(id, pos, spd, cur, ack)
        elif id == 2:
            # 使用锁机制，防止多线程同时操作
            with self.lock_msg_m2:
                # 设置电机2的位置、速度、电流和确认信息
                self.can_obj_motor2[0]=self.create_loc_controll_can_obj(id, pos, spd, cur, ack)
        else:
            # 如果id的值不是1或2，则抛出异常
            raise ValueError("id must be 1 or 2")

class Waist:
    
    def __init__(self):
        allDisabled = not (MotorCfg.waist.motor1.enabled or MotorCfg.waist.motor2.enabled)
        
        if allDisabled:
            print("MotorCfg: all waist motors are disabled")
            return
        
        self.motor = MotorWaist()
        self.motor2_angle = 0
        self.motor1_angle = 0
        for motor_id in range(1, len(MotorCfg.waist) + 1):
            if MotorCfg.waist[f"motor{motor_id}"].enabled:
                self.motor.motor_on(motor_id)
                print(f"waist motor{motor_id} enabled")

    def up(self, angle=1):
        self.motor2_angle+=angle
        print(f"motor1_angle:{self.motor1_angle}\n")
        print(f"motor2_angle:{self.motor2_angle}\n")
        self.set_motor_position(2, self.motor2_angle)
    
    def down(self, angle=1):
        self.motor2_angle-=angle
        print(f"motor1_angle:{self.motor1_angle}\n")
        print(f"motor2_angle:{self.motor2_angle}\n")
        self.set_motor_position(2, self.motor2_angle)

    def left(self, angle=1):
        self.motor1_angle+=angle
        print(f"motor1_angle:{self.motor1_angle}\n")
        print(f"motor2_angle:{self.motor2_angle}\n")
        self.set_motor_position(1, self.motor1_angle)
    
    def right(self, angle=1):
        self.motor1_angle-=angle
        print(f"motor1_angle:{self.motor1_angle}\n")
        print(f"motor2_angle:{self.motor2_angle}\n")
        self.set_motor_position(1, self.motor1_angle)

    def motor_on(self, motor_id):
        """
        打开电机
        :param motor_id: 电机编号
        """
        self.motor.motor_on(motor_id)

    def motor_off(self, motor_id):
        """
        关闭电机
        :param motor_id: 电机编号
        """
        self.motor.motor_off(motor_id)


    def set_motor_position(self, motor_id, pos=0., spd=3., cur=10, ack=2):
        """
        设置电机位置
        :param motor_id: 电机编号
        :param pos: 位置
        :param spd: 速度
        :param cur: 电流
        :param ack: 确认信息
        """
        self.motor.set_motor_position(motor_id, pos, spd, cur, ack)


if __name__ == '__main__':
    # motorWaist=MotorWaist(port='/dev/ttyUSB0')
    # motorWaist.inquire_position(1)
    waist = Waist() 
    
    # time.sleep(10)
    # print("go 10")
    # waist.set_motor_position(motor_id=1, pos=-10.)
    # waist.set_motor_position(motor_id=2, pos=-10.)
    # time.sleep(10)
    # print("go zero")
    # waist.set_motor_position(motor_id=1, pos=0.)
    # waist.set_motor_position(motor_id=2, pos=0.)
    # time.sleep(10)
    # print("go -10")
    # waist.set_motor_position(motor_id=1, pos=-10.)
    # waist.set_motor_position(motor_id=2, pos=-10.)
    # time.sleep(10)
    # print("go zero")
    # waist.set_motor_position(motor_id=1, pos=0.)
    # waist.set_motor_position(motor_id=2, pos=0.)
    

    